#include <math.h>
#include <unistd.h>

#include "lcm/lcm.h"

#include "common/getopt.h"
#include "common/timestamp.h"
#include "lcmtypes/diff_drive_t.h"

/* Example program showing how to publish on two different lcm
 * networks.  With the 2 lcm_t pointers, you can also subcribe to
 * channels on specific networks (not shown here)*/

int main(int argc, char *argv[])
{
    getopt_t *gopt = getopt_create();
    getopt_add_string(gopt, 'a', "channel-A", "A_DIFF_DRIVE", "LCM publish channel (ttl=0)");
    getopt_add_string(gopt, 'b', "channel-B", "B_DIFF_DRIVE", "LCM publish channel (ttl=1)");

    if (!getopt_parse(gopt, argc, argv, 1)) {
        printf("Options:\n");
        getopt_do_usage(gopt);
        return 0;
    }

    char *channels[2];
    channels[0] = getopt_get_string(gopt, "channel-A");
    channels[1] = getopt_get_string(gopt, "channel-B");
    char *urls[2];
    urls[0] = "udpm://239.255.76.67:7667?ttl=0";
    urls[1] = "udpm://239.255.76.68:7667?ttl=1";

    lcm_t *lcms[2];

    // the two networks must have either a different IP or port (IPs differ here)
    lcms[0] = lcm_create(urls[0]);  // same as NULL
    lcms[1] = lcm_create(urls[1]);

    diff_drive_t cmd = { .left = 0.0, .right = 0.0, .left_enabled = 0, .right_enabled = 0  };

    printf("Publishing on channels:\n(%s)\t%s\n(%s)\t%s\n", urls[0], channels[0],  urls[1], channels[1]);

    int i = 0;

    while(1) {
        usleep(100000);
        cmd.utime = timestamp_now();

        diff_drive_t_publish(lcms[i], channels[i], &cmd);
        i = (i + 1) % 2;
    }
}
